/*
 * File:   main.cpp
 * Author: dave
 *
 * Created on 2010年10月21日, 下午4:34
 */

#include <QtGui/QApplication>
#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>
#include <iostream>
#include <cxcore.h>
//#include<conio.h>
IplImage *image = 0, *hsv = 0, *hue = 0, *mask = 0, *backproject = 0, *histimg = 0;
CvHistogram *hist = 0;
int backproject_mode = 0;
int select_object = 0;
int track_object = 0;
int show_hist = 1;
CvPoint origin;
CvRect selection;
CvRect track_window;
CvBox2D track_box;
CvConnectedComp track_comp;
int hdims = 16;
float hranges_arr[] = {0,180};
float* hranges = hranges_arr;
int vmin = 10, vmax = 256, smin = 30;
IplImage* pFrame = NULL;
IplImage* pFrImg = NULL;
IplImage* pBkImg = NULL;
CvMat* pFrameMat = NULL;
CvMat* pFrMat = NULL;
CvMat* pBkMat = NULL;
CvCapture* pCapture = NULL;
CvCapture* capture = 0;
int nFrmNum = 0;

void MeanFilter(CvMat * src, CvMat * des, int s)
{
    int r = s >> 1;
    double sum;
    if(src->type != CV_32FC1) return;

    try
    {
        for(int i = r; i < src->height - r; ++i)
        {
            for(int j = r; j < src->width - r; ++j)
            {
                if(j == r)
                {
                    sum = 0;
                    for(int x = -r; x <= r; ++x)
                    {
                        for(int y = -r; y <= r; ++y)
                        {
                            sum += cvmGet(src, i + x, j + y);
                        }
                    }
                }
                else
                {
                    for(int y = -r; y <= r; ++y)
                    {
                        sum += cvmGet(src, i + r, j + y) - cvmGet(src, i - r - 1, j + y);
                    }
                }
                cvmSet(des, i, j, sum / s / s);
            }
        }
    }
    catch(...)
    {
        printf("均值滤波失败！");
    }
}

void locate()
{
    nFrmNum = 0;
    while(pFrame = cvQueryFrame( capture ))
    {
        nFrmNum++;
        
        if(nFrmNum == 1)
        {
            pBkImg = cvCreateImage(cvSize(pFrame->width, pFrame->height), IPL_DEPTH_8U,1);
            pFrImg = cvCreateImage(cvSize(pFrame->width, pFrame->height), IPL_DEPTH_8U,1);
            pBkMat = cvCreateMat(pFrame->height, pFrame->width, CV_32FC1);
            pFrMat = cvCreateMat(pFrame->height, pFrame->width, CV_32FC1);
            pFrameMat = cvCreateMat(pFrame->height, pFrame->width, CV_32FC1);

            cvCvtColor(pFrame, pBkImg, CV_BGR2GRAY);
            cvCvtColor(pFrame, pFrImg, CV_BGR2GRAY);
            cvConvert(pFrImg, pFrameMat);
            cvConvert(pFrImg, pFrMat);
            cvConvert(pFrImg, pBkMat);
        }
        else
        {
            cvCvtColor(pFrame, pFrImg, CV_BGR2GRAY);
            cvConvert(pFrImg, pFrameMat);

            cvAbsDiff(pFrameMat, pBkMat, pFrMat);

            cvThreshold(pFrMat, pFrImg, 60, 255.0, CV_THRESH_BINARY);

            cvErode(pFrImg, pFrImg, 0, 1);
            cvDilate(pFrImg, pFrImg, 0, 1);

            MeanFilter(pBkMat, pBkMat, 5);
            cvConvert(pBkMat, pBkImg);

            
//            pBkImg->origin=1;
//            pFrImg->origin=1;

            CvPoint pt1_Rect;
            CvPoint pt2_Rect;
            pt1_Rect.x=0;
            pt1_Rect.y=400;
            pt2_Rect.x=640;
            pt2_Rect.y=80;
            int thickness=1;
            int line_type=8;
            CvScalar color=CV_RGB(255,158,97);
            cvLine( pFrame, pt1_Rect, pt2_Rect, color,thickness, line_type, 0 );

            cvShowImage("background", pBkImg);
            cvShowImage("foreground", pFrImg);
            int x,y,b,cc;
            //int a[1200][800];
            y=120;
            for(;y<pFrImg->height;y+=240)
            {
                int k=0;
                for (x=5;x<pFrImg->width;x++)
                {
                    //  a[y][x]=(int)(((uchar*)(pFrImg->imageData+pFrImg->widthStep * y))[x]);
                    //  a[y][x]=*(pFrImg->imageData+pFrImg->widthStep*y+x);
                    //  if(a[y][x]!=0)
                    if((*(pFrImg->imageData+pFrImg->widthStep*y+x)) != 0)
                    {
                        cc=x;
                        x=pFrImg->width;
                        printf("有人进入");
                        printf("%d %d\n",cc,y);
                        b=640;
                        k=1;
                        break;
                    }
                }
                if(k)
                    break;
            }
            selection.x = cc-5;
            selection.y = y-5 ;
            selection.width = 10;
            selection.height = 10;

            printf("%d %d\n",selection.x,selection.y);
            
            track_object=-1;

            if(( cvWaitKey(27) >= 0 )||(b==640))
                break;
        }
    }
}

CvScalar hsv2rgb(float hue)
{
    int rgb[3], p, sector;
    static const int sector_data[][3] = {{0,2,1},{1,2,0},{1,0,2},{2,0,1},{2,1,0},{0,1,2}};
    hue *= 0.033333333333333333333333333333333f;
    sector = cvFloor(hue);
    p = cvRound(255*(hue - sector));
    p ^= sector & 1 ? 255 : 0;
    rgb[sector_data[sector][0]] = 255;
    rgb[sector_data[sector][1]] = 0;
    rgb[sector_data[sector][2]] = p;
    return cvScalar(rgb[2], rgb[1], rgb[0],0);
}

int main(int argc, char *argv[])
{
    char c;
//    printf("Do you want to start the detecting and tracking.('y' or 'n')\npress 'y' to start.\n");
//    if((c = getchar()) == 'y')
    {
        if(argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
            capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 );
        else
            capture = cvCaptureFromAVI("camera.avi");
        printf("argc=%d\n", argc);
        printf("argv[0]=%s\n",argv[0]);
        if( !capture )
        {
            fprintf(stderr,"Could not initialize capturing...\n");
            return -1;
        }
        printf( "Hot keys: \n"
                "\tESC - quit the program\n"
                "\tc - stop the tracking\n"
                "\tb - switch to/from backprojection view\n"
                "\th - show/hide object histogram\n"
                "To initialize tracking, select the object with mouse\n" );
        cvNamedWindow( "Histogram", 1 );
        cvNamedWindow( "CamShiftDemo", 1 );
        
        cvMoveWindow("Histogram", 0, 650);
        cvMoveWindow("CamShiftDemo", 810, 650);
        
        cvCreateTrackbar( "Vmin", "CamShiftDemo", &vmin, 256, 0 );
        cvCreateTrackbar( "Vmax", "CamShiftDemo", &vmax, 256, 0 );
        cvCreateTrackbar( "Smin", "CamShiftDemo", &smin, 256, 0 );

        cvNamedWindow("background",1);
        cvNamedWindow("foreground",1);

        cvMoveWindow("background", 0, 0);
        cvMoveWindow("foreground", 810, 0);
        
        for(;;)
        {
            locate();
            IplImage* frame = 0;
            int i, bin_w, c;
            frame = cvQueryFrame( capture );
            if( !frame )
                break;
            if( !image )
            {
                /* allocate all the buffers */
                image = cvCreateImage( cvGetSize(frame), 8, 3 );
                image->origin = frame->origin;
                hsv = cvCreateImage( cvGetSize(frame), 8, 3 );
                hue = cvCreateImage( cvGetSize(frame), 8, 1 );
                mask = cvCreateImage( cvGetSize(frame), 8, 1 );
                backproject = cvCreateImage( cvGetSize(frame), 8, 1 );
                hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );
                histimg = cvCreateImage( cvSize(320,200), 8, 3 );
                cvZero( histimg );
            }
            cvCopy( frame, image, 0 );
            cvCvtColor( image, hsv, CV_BGR2HSV );
            if( track_object )
            {
                int _vmin = vmin, _vmax = vmax;
                cvInRangeS( hsv, cvScalar(0,smin,MIN(_vmin,_vmax),0),
                        cvScalar(180,256,MAX(_vmin,_vmax),0), mask );
                cvSplit( hsv, hue, 0, 0, 0 );
                if( track_object < 0 )
                {
                    float max_val = 0.f;
                    cvSetImageROI( hue, selection );
                    cvSetImageROI( mask, selection );
                    cvCalcHist( &hue, hist, 0, mask );
                    cvGetMinMaxHistValue( hist, 0, &max_val, 0, 0 );
                    cvConvertScale( hist->bins, hist->bins, max_val ? 255. / max_val : 0., 0 );
                    cvResetImageROI( hue );
                    cvResetImageROI( mask );
                    track_window = selection;
                    track_object = 1;
                    cvZero( histimg );
                    bin_w = histimg->width / hdims;
                    for( i = 0; i < hdims; i++ )
                    {
                        int val = cvRound( cvGetReal1D(hist->bins,i)*histimg->height/255 );
                        CvScalar color = hsv2rgb(i*180.f/hdims);
                        cvRectangle( histimg, cvPoint(i*bin_w,histimg->height),
                                cvPoint((i+1)*bin_w,histimg->height - val),
                                color, -1, 8, 0 );
                    }
                }
                cvCalcBackProject( &hue, backproject, hist );
                cvAnd(backproject, mask, backproject, 0);
                cvCamShift(backproject, track_window,
                        cvTermCriteria(CV_TERMCRIT_EPS|CV_TERMCRIT_ITER, 10, 1),
                        &track_comp, &track_box);
                track_window = track_comp.rect;
                if( backproject_mode )
                    cvCvtColor( backproject, image, CV_GRAY2BGR );
                if( image->origin )
                    track_box.angle = -track_box.angle;
                cvEllipseBox( image, track_box, CV_RGB(255,0,0), 3, CV_AA, 0 );
            }
            if( select_object && selection.width > 0 && selection.height > 0 )
            {
                cvSetImageROI( image, selection );
                cvXorS( image, cvScalarAll(255), image, 0 );
                cvResetImageROI( image );
            }
            cvShowImage( "CamShiftDemo", image );
            cvShowImage( "Histogram", histimg );
            c = cvWaitKey(20);
            if( (char) c == 27 )
                break;
            switch( (char) c )
            {
                case 'b':
                    backproject_mode ^= 1;
                    break;
                case 'c':
                    track_object = 0;
                    cvZero( histimg );
                    break;
                case 'h':
                    show_hist ^= 1;
                    if( !show_hist )
                        cvDestroyWindow( "Histogram" );
                    else
                        cvNamedWindow( "Histogram", 1 );
                    break;
                default:;
            }
        }
        cvReleaseImage(&pFrImg);
        cvReleaseImage(&pBkImg);
        cvReleaseMat(&pFrameMat);
        cvReleaseMat(&pFrMat);
        cvReleaseMat(&pBkMat);
        cvReleaseCapture(&pCapture);
        cvReleaseCapture( &capture );
        cvDestroyWindow("CamShiftDemo");
    }
}
